
#include <Servo.h> 

//螺杆上下降的代码
#define IN11 31
#define IN12 33
#define IN13 35
#define IN14 37
#define EN1A 6
#define EN1B 7

//手部左右移动及旋转
#define IN31 22
#define IN32 24
#define IN33 26
#define IN34 28
#define EN3A 8
#define EN3B 9

//手部前后移动及行星齿轮转动
#define IN21 39
#define IN22 41
#define IN23 43
#define IN24 45

//洗发液的开放
#define IN41 47   
#define IN42 49
#define IN43 51
#define IN44 53

//舵机的初始化
Servo servo_1; // 左手大臂
Servo servo_2;  //左手小臂
Servo servo_3;  //右手大臂
Servo servo_4;  //右手小臂

//螺杆的上下降
void Descend(){
  digitalWrite(IN11,HIGH);      
  digitalWrite(IN13,HIGH);
  analogWrite(EN1A,180);
  analogWrite(EN1B,180);
  delay(4000);
  digitalWrite(IN11,LOW);
  digitalWrite(IN13,LOW);
  }
void Rise(){
  digitalWrite(IN12,HIGH);
  digitalWrite(IN14,HIGH);
  analogWrite(EN1A,200);
  analogWrite(EN1B,200);
  delay(5000);
  digitalWrite(IN12,LOW);
  digitalWrite(IN14,LOW);
  }

//手部螺杆前后移动
//往前移动
void qianhou1(){
  digitalWrite(IN24,HIGH);
  delay(16000);
  digitalWrite(IN24,LOW);
  }
//往后移动
void qianhou2(){
  digitalWrite(IN23,HIGH);
  delay(160000);
  digitalWrite(IN23,LOW);
  }
  
//喷洗发液的代码
void xifaye(){
  //喷洗发液
  digitalWrite(IN43,HIGH);
  delay(12000);
  digitalWrite(IN43,LOW);
  digitalWrite(IN41,HIGH);
  delay(5000);
  digitalWrite(IN41,LOW);
  }

//手部代码
void Rubbing1(){
  digitalWrite(IN21,HIGH);
  digitalWrite(IN31,HIGH);
  digitalWrite(IN34,HIGH);
  analogWrite(EN3A,200);     //圆周A方向运动
  analogWrite(EN3B,255);     //直线A方向运动
  delay(5000);
  analogWrite(EN3B,0);
  delay(2000);
  analogWrite(EN3A,0);
  digitalWrite(IN31,LOW);
  digitalWrite(IN34,LOW);
  digitalWrite(IN21,LOW);
  delay(500);
  }
  
void Rubbing2(){
  digitalWrite(IN22,HIGH);
  digitalWrite(IN32,HIGH);
  digitalWrite(IN33,HIGH);
  analogWrite(EN3A,200);     //圆周A方向运动
  analogWrite(EN3B,255);     //直线A方向运动
  delay(5000);
  analogWrite(EN3B,0);
  delay(2000);
  analogWrite(EN3A,0);
  digitalWrite(IN32,LOW);
  digitalWrite(IN33,LOW);
  digitalWrite(IN22,LOW);
  delay(500);
  }

void Rubbing1andDescend(){
  digitalWrite(IN21,HIGH);
  digitalWrite(IN31,HIGH);
  digitalWrite(IN34,HIGH);
  digitalWrite(IN11,HIGH);
  digitalWrite(IN13,HIGH);
  analogWrite(EN1A,200);
  analogWrite(EN1B,200);
  analogWrite(EN3A,200);     //圆周A方向运动
  analogWrite(EN3B,255);     //直线A方向运动
  delay(5000);
  analogWrite(EN3B,0);
  delay(2000);
  analogWrite(EN3A,0);
  digitalWrite(IN31,LOW);
  digitalWrite(IN34,LOW);
  digitalWrite(IN21,LOW);
  digitalWrite(IN11,LOW);
  digitalWrite(IN13,LOW);
  delay(500);
  }

void Rubbing2andDescend(){
  digitalWrite(IN22,HIGH);
  digitalWrite(IN32,HIGH);
  digitalWrite(IN33,HIGH);
  digitalWrite(IN11,HIGH);
  digitalWrite(IN13,HIGH);
  analogWrite(EN1A,200);
  analogWrite(EN1B,200);
  analogWrite(EN3A,200);     //圆周A方向运动
  analogWrite(EN3B,255);     //直线A方向运动
  delay(5000);
  analogWrite(EN3B,0);
  delay(2000);
  analogWrite(EN3A,0);
  digitalWrite(IN32,LOW);
  digitalWrite(IN33,LOW);
  digitalWrite(IN22,LOW);
  digitalWrite(IN11,LOW);
  digitalWrite(IN13,LOW);
  delay(500);
}
  
//总程序
//装置正运行
void Totalprogram(){
  Descend();
  qianhou1();
  tiaozheng();
  Rubbing2();
  Rubbing1();
  //喷水
  delay(10000);
  Rubbing2();
  Rubbing1();
  xifaye();
  Rubbing2();
  Rubbing1();
  //喷水
  delay(10000);
  Rubbing1andDescend();
  Rubbing2andDescend();
  }
//装置逆运行
void UnTotalprogram(){
  Rise();
  qianhou2();
  }
    
// the setup function runs once when you press reset or power the board
void setup() {
  // initialize digital pin LED_BUILTIN as an output.
  Serial.begin(9600);
  Serial2.begin(9600);
  
  //舵机引脚设置
  servo_1.attach(10); // 配置myservo实例的引脚为6
  servo_2.attach(11); // 配置myservo实例的引脚为9
  servo_3.attach(12); // 配置myservo实例的引脚为10
  servo_4.attach(13); // 配置myservo实例的引脚为11
  
  //初始化设置
  pinMode(IN11,OUTPUT);
  pinMode(IN12,OUTPUT);
  pinMode(IN13,OUTPUT);
  pinMode(IN14,OUTPUT);
  pinMode(IN21,OUTPUT);
  pinMode(IN22,OUTPUT);
  pinMode(IN23,OUTPUT);
  pinMode(IN24,OUTPUT);
  pinMode(IN31,OUTPUT);
  pinMode(IN32,OUTPUT);
  pinMode(IN33,OUTPUT);
  pinMode(IN34,OUTPUT);
  pinMode(IN41,OUTPUT);
  pinMode(IN42,OUTPUT);
  pinMode(IN43,OUTPUT);
  pinMode(IN44,OUTPUT);
  
  //初始化低电平
  digitalWrite(IN11,LOW);
  digitalWrite(IN12,LOW);
  digitalWrite(IN13,LOW);
  digitalWrite(IN14,LOW);
  
  digitalWrite(IN21,LOW);
  digitalWrite(IN22,LOW);
  digitalWrite(IN23,LOW);
  digitalWrite(IN24,LOW);

  digitalWrite(IN31,LOW);
  digitalWrite(IN32,LOW);
  digitalWrite(IN33,LOW);
  digitalWrite(IN34,LOW);

  digitalWrite(IN41,LOW);
  digitalWrite(IN42,LOW);
  digitalWrite(IN43,LOW);
  digitalWrite(IN44,LOW);
}

void tiaozheng(){
  servo_1.write(60);
  delay(1000);
  servo_2.write(50);
  delay(1000);
  servo_3.write(140);
  delay(1000);
  servo_4.write(120);
  delay(1000);
  delay(1000);
  //洗头动作
  servo_1.write(70);
  delay(1000);
  servo_2.write(80);
  delay(1000);
  servo_3.write(110);
  delay(1000);
  servo_4.write(100);
  delay(1000);
  }

// the loop function runs over and over again forever
void loop() {
  Totalprogram()                                                                                          ;
  delay(3000000);
  /*
  Totalprogram();
  delay(300000);
  qianhou1();
  tiaozheng();
  */
  Rubbing2();
  Rubbing1();
  Rubbing2();
  Rubbing1();
  delay(300000);
  UnTotalprogram();
  delay(1000000);
  String inString="";
  while(Serial2.available()>0){
    inString += char(Serial2.read());
    delay(10);      // 延时函数用于等待字符完全进入缓冲区，可以尝试没有延时，输出结果会是什么
  }
  // 检查是否接收到数据，如果接收到数据，则输出该数据
  if(inString=="11"){
    Totalprogram();
    delay(1000);
  } 
  if(inString=="12"){
    Serial.println("12");
    delay(1000);
  } 
  
}
